/*************************************************************************
*
*	simulate_attitude.c
*
*  Copyright 2007,2008 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides functions for simulating boat status.
*
*
**************************************************************************
*
*  version 1.0.8, 10/17/08, SBS
*   - broken out from simulate_boat
*
*  version 1.0.7, 10/4/08, SBS
*   - documentation
*
*  version 1.0.6, 8/27/08, SBS
*   - use status_SetTimeString()
*   - add random offset to boat start positions
*
*  version 1.0.5, 5/30/08, SBS
*    - move conversions (lat, long, deg, rad, etc) into conversions.h
*    - add conditionals for simulating buoys
*    - cleanup
*
*  version 1.0.4, 5/29/08, SBS
*    - add UpdateBoatSpeed()
*    - add status "direction" = net motion direction
*    - use direction instead of heading in positionfromheadingspeed
*    - move InitBoat here from simulate_engineering
*
*  version 1.0.3, 5/18/08, SBS
*    - add boat rotating into wind during free drift
*
*  version 1.0.2, 5/17/08, SBS
*    - modify drift to include both wind and water 
*    - add turn rate to heading changes
*
*  version 1.0.1, 5/12/08, SBS
*    - add drift
*
*  version 1.0, 1/13/08, SBS
*    - split from simulate_engineering.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @defgroup plat_eng_simulate
*	@ingroup plat_engineering
*  'Drivers' for simulated platform hardware
*/
/**
*  @file 
*  @ingroup plat_eng_simulate
*  @brief
*  This module provides functions for simulating boat status.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"
#include "conversions.h"

//#include "../initialize.h" 

#include "science_main.h"

#include "simulate_attitude.h"


// parameters for periodic changing of some telemetry bits

#define INITALT 0.1
#define MAXALT 1.0
#define MINALT -1.0
#define ALTINC 0.1

#define INITROLL 3.0
#define MAXROLL 10.0
#define MINROLL -10.0
#define ROLLINC 0.5

#define INITPITCH 2.0
#define MAXPITCH 10.0
#define MINPITCH -10.0
#define PITCHINC 0.5


/********************************************************
*
* initBoatOrientation()
*/
/**
* @brief
* Set initial pitch, roll, and altitude
*
********************************************************/
void InitBoatOrientation_sim(
			STATUS *s)
{
	s->dAltMeters = INITALT;       
	s->dPitchDeg = INITPITCH;           
	s->dRollDeg = INITROLL;           

	return;
}


/********************************************************
*
* UpdateBoatOrientation()
*/
/**
* @brief
* Simulates changes in pitch, roll, and altitude
*
********************************************************/
void UpdateBoatOrientation_sim(
			STATUS *s
			, int deltat)
{
	static double dPitchInc = PITCHINC;
	static double dRollInc = ROLLINC;
	static double dAltInc = ALTINC;		
	
	s->dAltMeters += dAltInc*deltat;
	s->dPitchDeg += dPitchInc*deltat;
	s->dRollDeg += dRollInc*deltat;
	
	if (s->dAltMeters > MAXALT) {
		
		dAltInc = -dAltInc;
		s->dAltMeters = MAXALT;
		
	}else if (s->dAltMeters < MINALT) {
		
		dAltInc = -dAltInc;
		s->dAltMeters = MINALT;
	}
	
	if (s->dPitchDeg < MINPITCH) {
	
		dPitchInc = -dPitchInc;
		s->dPitchDeg = MINPITCH;
		
	}else if (s->dPitchDeg > MAXPITCH) {
	
		dPitchInc = -dPitchInc;
		s->dPitchDeg = MAXPITCH;
	}
	
	if (s->dRollDeg < MINROLL) {
		
		dRollInc = -dRollInc;
		s->dRollDeg = MINROLL;
		
	}else if (s->dRollDeg > MAXROLL) {
		
		dRollInc = -dRollInc;
		s->dRollDeg = MAXROLL;
	}
	
	return;
}


